#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
Servo controller responsible for serial communication and auto-tracking.
"""

import logging
import threading
import time
from typing import Dict, Optional

import serial

from ...config.settings import GimbalConfig, SerialConfig, TrackingConfig
from ..tracking import ServoTracker
from .protocol import ServoProtocol

logger = logging.getLogger("RDK_YOLO")


class ServoController:
    """
    舵机控制器，封装串口通信、角度管理与自动追踪。
    """

    def __init__(
        self,
        gimbal_config: GimbalConfig,
        serial_config: SerialConfig,
        tracking_config: TrackingConfig,
    ):
        self.gimbal_config = gimbal_config
        self.serial_config = serial_config
        self.tracking_config = tracking_config

        self.serial: Optional[serial.Serial] = None
        self.connected = False
        self.lock = threading.Lock()

        self.current_pan = gimbal_config.init_pan_angle
        self.current_tilt = gimbal_config.init_tilt_angle

        self.protocol = ServoProtocol(gimbal_config)
        self.tracker = ServoTracker(self, tracking_config)

    # Connection management -------------------------------------------------

    def connect(self) -> bool:
        try:
            logger.info("尝试连接串口: %s, 波特率: %d", self.serial_config.port, self.serial_config.baudrate)
            self.serial = serial.Serial(self.serial_config.port, self.serial_config.baudrate, timeout=1)
            self.connected = True
            logger.info("已连接到串口设备")

            time.sleep(1.0)
            self._send_packet(ServoProtocol.build_test_packet())

            self.move_servos(self.gimbal_config.pan_pin, self.current_pan)
            time.sleep(0.2)
            self.move_servos(self.gimbal_config.tilt_pin, self.current_tilt)
            return True
        except Exception as e:
            logger.error("连接串口设备失败: %s", e)
            self.connected = False
            self.serial = None
            return False

    def disconnect(self) -> None:
        if self.serial and self.serial.is_open:
            self.serial.close()
        self.serial = None
        self.connected = False
        logger.info("已断开串口连接")

    def is_connected(self) -> bool:
        return self.connected

    # Movement --------------------------------------------------------------

    def move_servos(self, pin: int, angle: float) -> bool:
        return self.move_servos_together({pin: angle})

    def move_servos_together(self, servo_angles: Dict[int, float]) -> bool:
        if not self.connected or self.serial is None:
            logger.error("未连接到串口设备，无法控制舵机")
            return False

        servo_angles = {pin: max(0.0, min(270.0, angle)) for pin, angle in servo_angles.items()}

        if self.gimbal_config.pan_pin in servo_angles:
            self.current_pan = servo_angles[self.gimbal_config.pan_pin]
        if self.gimbal_config.tilt_pin in servo_angles:
            self.current_tilt = servo_angles[self.gimbal_config.tilt_pin]

        packet = self.protocol.build_packet(servo_angles)
        return self._send_packet(packet)

    def move_relative(self, pan_delta: float, tilt_delta: float) -> bool:
        targets = {}
        if pan_delta != 0:
            targets[self.gimbal_config.pan_pin] = max(
                self.gimbal_config.min_pan,
                min(self.gimbal_config.max_pan, self.current_pan + pan_delta),
            )
        if tilt_delta != 0:
            targets[self.gimbal_config.tilt_pin] = max(
                self.gimbal_config.min_tilt,
                min(self.gimbal_config.max_tilt, self.current_tilt + tilt_delta),
            )
        if not targets:
            return True
        return self.move_servos_together(targets)

    def _send_packet(self, packet: bytes) -> bool:
        with self.lock:
            try:
                logger.debug("发送舵机数据: %s", packet.hex().upper())
                bytes_written = self.serial.write(packet)
                logger.debug("已发送字节数: %d", bytes_written)
                time.sleep(0.05)
                return True
            except Exception as e:
                logger.error("发送舵机控制命令失败: %s", e)
                return False

    # Tracking --------------------------------------------------------------

    def start_auto_tracking(self, coord_calculator) -> bool:
        return self.tracker.start_tracking(coord_calculator)

    def stop_auto_tracking(self) -> bool:
        return self.tracker.stop_tracking()

    def update_tracking_target(self, *_args, **_kwargs) -> bool:
        """
        提供兼容接口，当前逻辑由 ServoTracker 控制，可在未来扩展。
        """
        return True
